#include "ocxo_ct.h"
#include "stdio.h"
#include "stm32f4xx_hal.h"
#include "kalman.h"
#include "main.h"
#include "GP21.h"
#include <string.h>
#include "config.h"
#include "eeprom.h"
#include "Utility.h"
#include "mainlogic.h"
#include "spi_flash.h"
#include "DBG.h"
#include "LinearRegression.h"
#include "PID.h"
#include "stm32f4xx_hal_rtc_ex.h"

static ocxo_ct_status g_ctStatus = OCT_Wait;
static uint32_t g_sampleCount;
static int64_t g_sumValue;
static int64_t g_ctLowValue = 0;
//ct
static int g_init;
extern uint8_t gpsUpdated;
extern TIM_HandleTypeDef htim1;
extern KalmanData kalman;

static History_Data history24hour[24];
static uint8_t history24hour_idx = 0;
static uint8_t history24hour_full = 0;

static History_Data history30Day[30];
static uint8_t history30Day_idx = 0;
static uint8_t history30Day_full = 0;



typedef struct
{
	uint32_t dacvalue;
	float temp;
	uint32_t time;
	float offset;
} OcxoLog;

OCXO_Status g_oc_status = {0}; 

extern int qErrorOff;

static double det_center = 500000;
static uint32_t det_span = 10000;
#define CON_CT 30

void setDacValue(int32_t value, uint8_t offsetModel);
void doNewOffset(int32_t offset, int32_t tdc);
void adjustOcxo(int32_t offset, int32_t tdc);
uint8_t ocxo_spiflash_logInit(void);
PID_Controller pid;

#define ABS(x) ((x)>0?(x):(-(x)))

void Stop_OC_Control(uint8_t stop)
{
	g_oc_status.stopControl = stop;
}

typedef struct {
    int timestamp;    // 时间戳
    float temperature;   // 温度
    float sensor_value;  // 传感器数值
} SensorData;

/*SensorData debug_data[45] = {{42,31.3125f,4380929.0000f}, {43,31.3125f,4380894.5000f}, {44,31.3125f,4380859.5000f}, {45,31.3125f,4380820.5000f}, {46,31.3125f,4380834.5000f},
    {47,31.3125f,4380813.5000f}, {48,31.3125f,4380892.5000f}, {49,31.3125f,4380735.0000f}, {50,31.3125f,4380750.0000f}, {51,31.3125f,4380641.0000f},
    {52,31.3125f,4380554.5000f}, {53,31.3125f,4380546.5000f}, {54,31.3125f,4380521.0000f}, {55,31.3125f,4380501.5000f}, {56,31.2500f,4380410.0000f},
    {57,31.2500f,4380404.5000f}, {58,31.2500f,4380415.5000f}, {59,31.2500f,4380677.5000f}, {60,31.2500f,4380739.5000f}, {61,31.2500f,4380777.5000f},
    {62,31.2500f,4380792.0000f}, {63,31.2500f,4380782.5000f}, {64,31.2500f,4380962.5000f}, {65,31.2500f,4380841.5000f}, {66,31.2500f,4380731.0000f},
    {67,31.2500f,4380703.0000f}, {68,31.2500f,4380898.5000f}, {69,31.2500f,4380886.5000f}, {70,31.2500f,4380876.0000f}, {71,31.2500f,4380819.5000f},
    {72,31.2500f,4381007.5000f}, {73,31.2500f,4380908.0000f}, {74,31.2500f,4380711.0000f}, {75,31.2500f,4380913.0000f}, {76,31.2500f,4380879.0000f},
    {77,31.2500f,4380798.0000f}, {78,31.2500f,4380798.0000f}, {79,31.2500f,4380853.5000f}, {80,31.3125f,4380760.5000f}, {81,31.3125f,4380728.0000f},
    {82,31.3125f,4380607.5000f}, {83,31.3125f,4380525.5000f}, {84,31.3750f,4380451.0000f}, {85,31.3750f,4380513.0000f}, {86,31.3750f,4380504.5000f}};

void TempDebug(void)
{
	LinearRegressionData tt;
	linear_regression_init(&tt);
	for(int i=0;i<45;i++)
	{
		linear_regression_update(&tt, debug_data[i].temperature, debug_data[i].sensor_value);
	}
	double temp;
	linear_regression_calculate(&tt, &temp);
	DBGI("*********************************tempcof = %f agecof = %f\n", temp);
}*/

void intOCXO_ct(void) 
{
	g_ctStatus = OCT_Wait;
	g_sampleCount = 0;
	g_sumValue = 0;
	det_center = g_Ocxo_info.dacValue;
	g_oc_status.needSyncPPS = 0;
	if(g_Ocxo_info.adjValid && ABS(g_Ocxo_info.adj_step)>0.1f)
	{
		g_ctStatus = OCT_Fine;
		setDacValue(g_Ocxo_info.dacValue, 0);
		g_oc_status.needSyncPPS = 1;
		DBGI("ocxo ref step=%f, dac=%d\n", g_Ocxo_info.adj_step, g_Ocxo_info.dacValue);
	}
	ocxo_spiflash_logInit();
	//TempDebug();
  // 初始化PID：kp=2.0, ki=0.5, kd=0.1，输出限制[-100,100]，积分限幅50
  pid_init(&pid, 2.0f, 0.5f, 0.1f, 100.0f, 65535.0f*128, 45535.0f*128);
	g_oc_status.offsetInvlid = 0;
    
  // 设置目标温度为50度
  pid_set_setpoint(&pid, 7000.0f);
}

void ocxo_detectSet(double center, uint32_t span)
{
	g_sampleCount = 0;
	det_center = center;
	det_span = span;
	g_sumValue = 0;
	g_ctLowValue = 0;
	g_ctStatus = OCT_DetectCT_LOW;
}

void ocxo_doDetectCmd(uint32_t span)
{
	ocxo_detectSet(g_Ocxo_info.dacValue, span);
}

void Ocxo_StartDetect(void)
{
		g_sampleCount = 0;
		g_sumValue = 0;
		g_init = 1;
		setDacValue(g_Ocxo_info.dacValue,0);
		g_oc_status.historyIdx = 0;
		g_oc_status.historySum = 0;
		g_oc_status.history10MinSum = 0;
		g_oc_status.history10MinOk = 0;
		g_oc_status.controlCount = 0;
		g_oc_status.controlSum = 0;
		g_oc_status.historyOk = 0;
		g_oc_status.fixed = 0;
		g_oc_status.fixedCT = 0;
	  g_oc_status.needSyncPPS = 1;
		kalman.seconds = kalman.seconds - kalman.seconds%30;
}

void ocxoct_findcenter(int32_t value)
{
	#define CENTER 415000
	static int error = 0;
	static int ct = 0;
	static uint8_t init = 0;
	static int32_t center = CENTER;
	static int32_t span = 0;
	static int8_t flag = 1;
	DBGI("find center status value = %d\n", value/76);
	if(!init)
	{
		center = CENTER;
		DBGI("set center to %d\n",center);
		setDacValue(center,0);
		init = 1;
		error = 0;
		ct = 0;
		span = 0;
	}
	else {
		DBGI("error = %d ct=%d span=%d flag=%d\n", error, ct, span, flag);
		if(ABS(value) > ADC2PS * 400) {
			error++;
		}
		ct++;
		if(ct > 6 || ABS(value)> ADC2PS * 3000 || error >= 2)
		{
			if(error < 2 && ct>6)
			{
				g_Ocxo_info.dacValue = center;
				ocxo_detectSet(center, 10000);
				init = 0;
			}
			else {
				if(flag > 0)
					span += 3000;
				center = CENTER + flag * span;
				DBGI("------------------set center to %d flag=%d\n",center, flag);
				flag = -flag;
				if(center>0)
				{
					setDacValue(center,0);
					error = 0;
					ct = 0;
				}
				if(center >= 1000000)
				{
					g_ctStatus = OCT_ERROR;
					init = 0;
				}
			}
		}
	}
}

void ocxoct_detectCt(int32_t value) {
	double tmp;
	double tmp2;
	static int ct = 0;
	if(ABS(value) > ADC2PS * 1300) {
		ct++;
	}
	else 
	{
		ct = 0;
	}
	if(ct > 5)
	{
		g_ctStatus = OCT_FIND_CENTER;
		ct = 0;
		DBGI("change to OCT_FIND_CENTER status\n");
	}
	DBGI("detect ct value offset %d sample=%u\n",value,g_sampleCount);
	if(g_sampleCount == 0) {
		uint32_t dac = det_center;
		if(g_ctStatus == OCT_DetectCT_LOW) {
			setDacValue((dac-det_span),0);
		} else if(g_ctStatus == OCT_DetectCT_HIGH) {
			setDacValue((dac+det_span),0);
		}
		g_sampleCount++;
	} 
	else 
	{
    g_sampleCount++;
		g_sumValue += value;
		if(g_sampleCount == 101){
			if(g_ctStatus == OCT_DetectCT_LOW) 
			{
				g_ctStatus = OCT_DetectCT_HIGH;
				g_sampleCount = 0;		
				g_ctLowValue = g_sumValue;
				g_sumValue = 0;
				DBGI("goto status OCT_DetectCT_HIGH....\r\n");
			} 
			else 
			{
				g_ctStatus = OCT_Fine;
				tmp = 2 * det_span;
				tmp2 = g_sumValue-g_ctLowValue;
				//tmp2因为是100次累加结果，所以需要除与100
				tmp2 = tmp2/(100.0);
				tmp2 = tmp2;
				double step = tmp/tmp2;	
				DBGI(">>>>>>>>>>>>>>>>>>>>>>>>>>high=%lld low=%lld offset=%lld %lf %lf ctstep is %lf\r\n",g_sumValue,g_ctLowValue,g_sumValue-g_ctLowValue,tmp2,tmp*step,step);
				OcxoInfo_SetStep(step, det_center);
        Ocxo_StartDetect();
				kalmanUpdateRate(&kalman);
			}
		}
	}
}

void syncOcxo(int32_t tdc)
{
	
	static uint16_t ct = 0;
	DBGI("SYNC>> tdc=%d ct=%d\n", tdc, ct);
	if(ct == 0)
	{
		float tmp = g_Ocxo_info.adj_step;
		if(ABS(tmp)<0.1f) tmp = 5;
		setDacValue(g_Ocxo_info.dacValue - 2000000/tmp * (tdc>0?1:-1), 0);
	}
	if(ABS(tdc) < 500 || ct > 1024)
	{
		g_ctStatus = OCT_Fine;
		Ocxo_StartDetect();
	}
	else {
		ct++;
	}
}

void updateSensor(int32_t value, int32_t tdc) {
	switch(g_ctStatus)
	{
		case OCT_Wait:
		{
			DBGI("wait status.... offset = %d remain=%u\r\n", value,WAIT_TIME-g_sampleCount);
			g_sampleCount++;
			if(g_sampleCount>WAIT_TIME) {
				DBGI("goto status OCT_DetectCT_LOW....\r\n");
				g_ctStatus = OCT_DetectCT_LOW;
				g_sampleCount = 0;
			}
			break;
		}
		case OCT_ERROR:
		{
			DBGI("OCT_ERROR\n");
			break;
		}
		case OCT_FIND_CENTER:
		{
			ocxoct_findcenter(value);
			break;
		}
		case OCT_DetectCT_LOW:
		{
			ocxoct_detectCt(value);
			break;
		}
		case OCT_DetectCT_HIGH:
		{
			ocxoct_detectCt(value);
			break;
		}
		case OCT_Crose:
			break;
		case OCT_Fine:
		{
			adjustOcxo(value, tdc);
			/*
			if(kalman.seconds == 600 && !init)
			{
				init = 1;
				g_ctStatus = OCT_SYNC;
			}
			else {
				adjustOcxo(value, tdc);
			}*/
		}
			break;
		case OCT_SYNC:
			syncOcxo(tdc);
			break;
		default:
			break;
	}
}

extern IWDG_HandleTypeDef hiwdg;

void ocxo_eeprom_log(void)
{
	//eeprom共8k，其中前64byte做配置，共可存储 8128/8 = 1016项记录
	uint32_t addr = 64 + g_Ocxo_info.historyCount * 8;
	if(g_Ocxo_info.historyCount < 1016)
	{
		uint8_t *p = (uint8_t *)(&g_Ocxo_info.dacValue);
		HAL_StatusTypeDef error;
		for(int i=0;i<8;i++)
		{
			AA24x_WriteOneByte(addr, *(p+i), &error);
		}
		g_Ocxo_info.historyCount++;
	}
	g_Ocxo_info.time = mktime(g_gpsStatus.year, g_gpsStatus.month, g_gpsStatus.day, g_gpsStatus.hour,g_gpsStatus.min, g_gpsStatus.sec);
	OcxoInfo_Save();
}

#define SECTOR_BYTES 4096
//#define SECTOR_COUNT 512
#define IDX_SECTOR_COUNT 16 
#define TOTAL_LOG_COUNT ((g_flash_sector_count-IDX_SECTOR_COUNT)*(SECTOR_BYTES/16))

void ocxo_logshow(uint16_t start, uint16_t end)
{
	for(int i=start;i<end;i++)
	{
		uint32_t addr = i * 16 + SECTOR_BYTES*IDX_SECTOR_COUNT;
		OcxoLog tmp;
		uint8_t *buf = (uint8_t *)&tmp;
		SPI_Flash_ReadUnfixed(buf, addr, 16);
	  if(tmp.time != 0xFFFFFFFF)
		{
			Calendar_Structure datetime;
			TimestampToNormalTime(&datetime, tmp.time+8*60*60);
			double realFreq = 10000000.0 + (double)tmp.offset * 0.00001;
			DBGI("temp=%.2f freq=%lf dac=%d %d %d-%02d-%02d %02d:%02d:%02d of=%f\n", tmp.temp, realFreq, 
				tmp.dacvalue,tmp.dacvalue>>7, datetime.year, datetime.month, datetime.day , datetime.hour, datetime.min, 
				datetime.second, tmp.offset);
			vTaskDelay(100);
		}
		else
		{
			DBGI("log is empty\n");
			break;
		}
		HAL_IWDG_Refresh(&hiwdg);
	}
}

float TimeUpdateRate(void)
{
	float ret = 1;
	extern RTC_HandleTypeDef hrtc;
	uint32_t now = mktime(g_gpsStatus.year, g_gpsStatus.month, g_gpsStatus.day, g_gpsStatus.hour,g_gpsStatus.min, g_gpsStatus.sec);
	uint32_t backup = HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR0);
	if(backup == 0)
	{
		OcxoLog cur;
		int32_t idx = g_oc_status.spiLogIdx -1;
		uint32_t addr = idx * 16 + SECTOR_BYTES*IDX_SECTOR_COUNT;
		uint8_t *buf = (uint8_t *)&cur;
		SPI_Flash_ReadUnfixed(buf, addr, 16);
		if(cur.time != 0xFFFFFFFF)
		{
			DBGI("backtime from log... time=%d\n", cur.time);
			backup = cur.time;
		}
	}
	else {
		DBGI("backtime from RTC Backup.\n");
	}
	uint32_t offset = now - g_Ocxo_info.time;
	DBGI("offset time = %d\n", offset);
	if(offset > 3600 * 10)
	{
		ret = 2;
	}
	else if(offset > 3600 * 5)
	{
		ret = 1;
	}
	else if(offset > 3600 * 1)
	{
		ret = 0.5f;
	}
	else if(offset > 60)
	{
		ret = 0.2f;
	}
	else
	{
		ret = 0.1f;
	}
	DBGI("update rate = %f offset=%d\n", ret, offset);
	return ret;
}

void ocxo_showlog_age(uint16_t span)
{
	DBGI("ocxo_showlog_age name=%s idx=%d span=%d\n", g_Ocxo_info.ocxo_info, g_oc_status.spiLogIdx, span);
	int32_t idx = g_oc_status.spiLogIdx -1;
	OcxoLog cur;
	OcxoLog pre;
	for(int i=0; i<= idx ;i = i + span)
	{
		uint32_t addr = i * 16 + SECTOR_BYTES*IDX_SECTOR_COUNT;
		uint8_t *buf = (uint8_t *)&cur;
		SPI_Flash_ReadUnfixed(buf, addr, 16);
	  if(cur.time != 0xFFFFFFFF)
		{
			if(i != 0)
			{
				Calendar_Structure datetime;
				TimestampToNormalTime(&datetime, cur.time+8*60*60);
				int32_t age = cur.dacvalue - pre.dacvalue;
				float ageReg = age / g_Ocxo_info.adj_step / 1000;
				DBGI("%d-%02d-%02d %02d:%02d:%02d - tmp=%f %f pre=%d cur=%d age=%d ageReg=%.4f ppb\n", datetime.year, datetime.month, datetime.day , datetime.hour, datetime.min, datetime.second, 
					pre.temp, cur.temp, pre.dacvalue,cur.dacvalue, age, ageReg);
			}
			pre = cur;
		}
		else {
			DBGI("end\n");
			break;
		}
	}
}

void ocxo_spiflash_log(void)
{
	static OcxoLog tmp;
	tmp.dacvalue = g_Ocxo_info.dacValue;
	tmp.offset = g_oc_status.offset10Min;
	tmp.temp = g_ctlStatus.temp;
	tmp.time = mktime(g_gpsStatus.year, g_gpsStatus.month, g_gpsStatus.day, g_gpsStatus.hour,g_gpsStatus.min, g_gpsStatus.sec);
	g_oc_status.map = g_oc_status.map & (~(1<<g_oc_status.idx));
	g_oc_status.idx++;
	SPI_Flash_WriteUnfixed((uint8_t *)&tmp,(g_oc_status.spiLogIdx*16 + SECTOR_BYTES*IDX_SECTOR_COUNT),16);
	SPI_Flash_WriteUnfixed(&g_oc_status.map,((g_oc_status.spiLogIdx>>3)+1),1);
	if(g_oc_status.idx == 8)
	{
		g_oc_status.idx = 0;
		g_oc_status.map = 0xFF;
	}
	g_oc_status.spiLogIdx++;
	DBGI("write spi log %d\n", g_oc_status.spiLogIdx);
	//DBGI("%d\n", g_oc_status.spiLogIdx);
	//索引区需要擦除
	if((g_oc_status.map == 0xFF) && ((((g_oc_status.spiLogIdx>>3)+1) % (SECTOR_BYTES) == 0) || (g_oc_status.spiLogIdx >= TOTAL_LOG_COUNT)))
	{
		uint32_t sector = (g_oc_status.spiLogIdx+1) / SECTOR_BYTES;
		//31个block, 每个block有16个sector
		if(g_oc_status.spiLogIdx >= TOTAL_LOG_COUNT) 
		{
			g_oc_status.spiLogIdx = 0;
			sector = 0;
			DBGI("data is full reset\n");
		}
		else {
			sector = ((g_oc_status.spiLogIdx>>3)+1) / SECTOR_BYTES;
		}
		SPI_Flash_EraseSector(sector*SECTOR_BYTES);	
		uint8_t t = 0xAA;
		SPI_Flash_WriteUnfixed(&t,0,1);
		DBGI("idx EraseSector %d\n", sector);
	}
	if(g_oc_status.spiLogIdx % (SECTOR_BYTES/16) == 0)
	{
		uint32_t sector = g_oc_status.spiLogIdx / (SECTOR_BYTES/16) + IDX_SECTOR_COUNT;  //(*32/256)
		SPI_Flash_EraseSector((sector) * SECTOR_BYTES);	
		DBGI("EraseSector %d\n", sector);
	}
}

void ocxo_spiflash_printidx(uint32_t count)
{
	uint8_t buf[256];
	uint32_t ct = 0;
	for(int j=0;j<256;j++)
	{
		printf("\n>>> %d\n", j);
		SPI_Flash_ReadUnfixed(buf,j*256,256);
		for(int i=0;i<256;i++)
		{
			printf("%x ",buf[i]);
			ct++;
			if(ct == count)
			{
				return;
			}
		}
		printf("\n");
		HAL_IWDG_Refresh(&hiwdg);
	}
}

void ocxo_spiflash_test(void)
{
	int count =  TOTAL_LOG_COUNT+10;//128*16*31*2 + 10;
	for(int i=0;i<count;i++)
	{
		g_Ocxo_info.dacValue = i;
		ocxo_spiflash_log();
		HAL_IWDG_Refresh(&hiwdg);
	}
	ocxo_spiflash_printidx(count);
	ocxo_logshow(0,count);
}	
//16Mflash一共有32个block,第一个block存储索引，第一个byte 为0xAA, 如果不符则擦除整个flash
uint8_t ocxo_spiflash_logInit(void)
{
	uint8_t buft[256];
	uint8_t ret = 1;
	uint32_t idx = 0;
	uint16_t localIdx = 0;
	uint8_t end = 0;
	g_oc_status.map = 0xFF;
	g_oc_status.idx = 0;
	DBGI("spi flash log init.\n");
	for(int j=0;j<256;j++)
	{
		ret = 1;
		localIdx = 0;
		printf(".\n");
		SPI_Flash_ReadUnfixed(buft,j*256,256);
		//检查是否经过初始化
		if(idx == 0 && buft[0] != 0xAA)
		{
			ret = 0;
			DBGI("Init Erase falsh memory\n");
			SPI_Flash_EraseTotal();
			buft[0] = 0xAA;
			SPI_Flash_WriteUnfixed(buft,0,1);
			g_oc_status.spiLogIdx = 0;
			g_oc_status.spiLogInit = 1;
			DBGI("Init Erase falsh memory end\n");
			return 1;
		}
		if(j == 0)
		{
			localIdx = 1;
		}
		for(int i=0;i<256;i++)
		{
			printf("%x ", buft[i]);
		}
		printf("\n");
		for(;localIdx<256;localIdx++)
		{
			if(buft[localIdx] != 0x0)
			{
				g_oc_status.map = buft[localIdx];
				switch(buft[localIdx]) {
					case 0xFE:
						g_oc_status.idx = 1;
						idx+= 1;
						break;
					case 0xFC:
						g_oc_status.idx = 2;
						idx+=2;
						break;
					case 0xF8:
						g_oc_status.idx = 3;
						idx+=3;
						break;
					case 0xF0:
						g_oc_status.idx = 4;
						idx+=4;
						break;
					case 0xE0:
						g_oc_status.idx = 5;
						idx+=5;
						break;
					case 0xC0:
						g_oc_status.idx = 6;
						idx+=6;
						break;
					case 0x80:
						g_oc_status.idx = 7;
						idx+=7;
						break;
					default:
						g_oc_status.idx = 0;
						break;
				}
				end = 1;
				printf("find end ct=%d idx=%d map=%x\n",idx, g_oc_status.idx, g_oc_status.map);
				break;
			}
			else {
				idx+=8;
			}
			if(idx >= TOTAL_LOG_COUNT)
			{
				printf("\nspiflash full = %d\n",idx);
				break;
			}
		}
		if(end)
		{
			g_oc_status.spiLogIdx = idx;
			g_oc_status.spiLogInit = 1;

			printf("\nspiflash init addr = %d\n",idx);
			break;
		}
		HAL_IWDG_Refresh(&hiwdg);
	}
	if(!end)
	{
		printf("\nspiflash Erase first Sector\n");
		SPI_Flash_EraseSector(0);
		SPI_Flash_EraseSector(16);
		g_oc_status.spiLogIdx = 0;
		g_oc_status.spiLogInit = 1;
	}
	return ret;
}

void ocxo_log(void)
{
	extern RTC_HandleTypeDef hrtc;
	uint32_t yu10s = kalman.seconds % 60;
	if(kalman.seconds%600 == 0)
	{
			ocxo_spiflash_log();
	}
	//spi flash 每60秒记录一次
	if(yu10s == 0)
	{
		uint32_t now = mktime(g_gpsStatus.year, g_gpsStatus.month, g_gpsStatus.day, g_gpsStatus.hour,g_gpsStatus.min, g_gpsStatus.sec);
		HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR0, now);
		//eeprom中记录时间间隔为 1，2,3....10分钟内没一分钟记录一次，10-60分钟内每10分钟记录一次，之后30分钟记录一次
		if(kalman.seconds <=600)
		{
			ocxo_eeprom_log();
		}
		else if(kalman.seconds <= 3600)
		{
			
			uint32_t t = kalman.seconds % 600;
			if(t==0)
			{
				ocxo_eeprom_log();
			}
		}
		else
		{
			uint32_t t = kalman.seconds % 18800;
			if(t==0)
			{
				ocxo_eeprom_log();
			}
		}
	}
}



void TempAndAge(void)
{
	static LinearRegressionData tempData;
	static LinearRegressionData age24hour;
	static int first = 0;
	static int onehour = 0;
	if(g_oc_status.fixed && !first)
	{
		first = 1;
		linear_regression_init(&tempData);
		linear_regression_init(&age24hour);
		tempData.avg = 3204398;
		age24hour.avg = 3204398;
	}
	if(first)
	{
		if(tempData.count < 120)
		{
			linear_regression_update(&tempData, g_ctlStatus.temp,g_Ocxo_info.dacValue); 
		}
		else {
			double temp,age;
			linear_regression_calculate(&tempData, &temp);
			DBGI("*********************************tempcof = %lf agecof = %lf\n", temp,age);
			g_ctlStatus.cofeValid = 1;
			g_ctlStatus.tempCofe = temp;
			g_ctlStatus.ageCofe = age;
			g_ctlStatus.avg = tempData.avg;
			linear_regression_init(&tempData);
			age24hour.avg = tempData.avg;
			onehour = 1;
		}
		if(onehour)
		{
			if(age24hour.count < 120*24)
			{
				linear_regression_update(&age24hour, g_ctlStatus.temp, g_Ocxo_info.dacValue);
			}
			else {
				double temp,age;
				linear_regression_calculate(&age24hour, &temp);
				DBGI("*********************************tempcof = %lf agecof = %lf\n", temp,age);
				g_ctlStatus.cofe24Valid = 1;
				g_ctlStatus.tempCofe24 = temp;
				g_ctlStatus.ageCofe24 = age;
				linear_regression_init(&age24hour);
			}
			if(g_ctlStatus.cofe24Valid)
			{
				DBGI("24hour cofe temp = %.4lf, age = %.4lf\n", g_ctlStatus.tempCofe24, g_ctlStatus.ageCofe24);
			}
			if(g_ctlStatus.cofeValid)
			{
				DBGI("1hour cofe temp = %.4lf, age = %.4lf\n", g_ctlStatus.tempCofe, g_ctlStatus.ageCofe);
			}
		}
	}
	
}

void historylog(void)
{
	uint32_t hour = kalman.seconds / 3600;
	uint32_t yu = kalman.seconds - hour*3600;
	if(hour>0 && yu == 0)
	{
		history24hour[history24hour_idx].dac = g_Ocxo_info.dacValue;
		history24hour[history24hour_idx].temp = g_ctlStatus.temp;
		history24hour_idx++;
		if(history24hour_idx >= 24)
		{
			history24hour_full = 1;
			history24hour_idx = 0; 
			history30Day[history30Day_idx].dac = g_Ocxo_info.dacValue;
			history30Day[history30Day_idx].temp = g_ctlStatus.temp;
			history30Day_idx++;
			if(history30Day_idx >= 30)
			{
				history30Day_idx = 0;
				history30Day_full = 1;
			}
		}
	}
}

void printHistoryItem(uint8_t i, History_Data*first, History_Data* second, History_Data* pre)
{
	double offset = second->dac - first->dac;
	double of2 = offset / g_Ocxo_info.adj_step;
	double preoff = of2/i;
	if(pre)
	{
		preoff = (second->dac-pre->dac)/ g_Ocxo_info.adj_step;
	}
  	DBGI("idx=%d t_off=%.1f t2=%.1f v1=%.1lf v2=%.1lf offset=%.1lf of2=%.1lf of3=%.1lf pre=%.1lf ppb=%.4f\n",
	 	i , first->temp, second->temp, first->dac, second->dac, offset, of2, of2/i, preoff, preoff/1000.0f);
	vTaskDelay(100);
}

void printHistory(History_Data *dataList, uint8_t count, uint32_t idx, uint32_t full)
{
	uint8_t nodata = 0;
	nodata = (!full) && ((idx == 0));
	if(!nodata)
	{
		uint8_t ct = 0;
		History_Data *first;
		History_Data *second;
		History_Data *pre = NULL;
		if(full)
		{
			ct = idx;
			for(int i=0;i<count;i++)
			{
				if(i==0)
				{
					first = dataList+ct;
				}
				else 
				{
					second = dataList+ct;
					printHistoryItem(i, first, second, pre);
					pre = second;
				}
				ct++;
				if(ct >= count)
					ct = 0;
			}
		}
		else 
		{
			for(int i=0;i<idx;i++)
			{
				if(i==0)
				{
					first = dataList;
				}
				else {
					second = dataList+i;
					printHistoryItem(i, first, second, pre);
					pre = second;
				}
			}
		}
	}
	else {
		DBGI("nodata\n");
	}
}

void cmd_showHistoryLog(void)
{
	DBGI("%s >>> history log.\n", g_Ocxo_info.ocxo_info);
	DBGI("print history 24hour.\n");
	printHistory(history24hour, 24, history24hour_idx, history24hour_full);
	
	DBGI("print history 30day.\n");
	printHistory(history30Day, 30, history30Day_idx, history30Day_full);
}

void adjustOcxo(int32_t offset, int32_t tdc) {
	static uint8_t errorCt = 0;
	if(g_init>0) {
		g_init--;
		g_oc_status.controlSum = 0;
		g_oc_status.historyIdx = 0;
		g_oc_status.historyOk = 0;
		g_oc_status.controlCount = 0;
		return;
	}
	float offsetPs = offset;  //将ms1022计数值转换为ps单位 （一个计数=200us/65536)
	float fixOffset = offsetPs;
	if(ABS(offset) > ADC2PS * 3000)
	{
		errorCt++;
		if(errorCt > 5)
		{
			g_ctStatus = OCT_FIND_CENTER;
			DBGI("change to OCT_FIND_CENTER status\n");
			return;
		}
	}
	if(g_Controller_config.qError)
	{
		fixOffset	+= qErrorOff;  //time plus 修正
	}
	qErrorOff = 0;
	g_oc_status.controlSum += fixOffset;
	g_oc_status.controlCount++;
	kalmanUpdate(&kalman,g_oc_status.fixed);
	float avg = g_oc_status.controlSum / g_oc_status.controlCount;
	//printf("%.1f %d %.1f\n", controlSum, controlCount, avg);
	int8_t idx = CON_CT - g_oc_status.controlCount;
	if(idx < 5)
	{
		g_oc_status.controlDatas[idx] = avg;
	}
	//DBGI(">>>>off=%6d %6.1f fixOffps=%.1f avg=%.1f ct=%d\n", offset,offsetPs, fixOffset,avg,g_oc_status.controlCount);
	if(g_oc_status.controlCount > 25)
	{
		DBGI(">>>>ct=%d off=%.1f avg=%.1f \n", g_oc_status.controlCount, fixOffset,avg);
	}
	if(idx <= 0)
	{
		errorCt = 0;
		float min = 9999999;
		for(int i=0;i<5;i++)
		{
			if(ABS(min) > ABS(g_oc_status.controlDatas[i]))
			{
				min = g_oc_status.controlDatas[i];
			}
		}
		double kalmantmp = min;
		//偏差大于5000时 不做滤波处理
		if(ABS(min) < 5000)
		{
			kalmantmp = kalmanFilter(&kalman,min);
			//判断是否锁定
			if(ABS(min)<500)
			{
				g_oc_status.fixedCT++;
			}
			else
			{
				g_oc_status.fixedCT = 0;
			}
			if(g_oc_status.fixedCT>=10)
			{
				if(!g_oc_status.fixed)
				{
					g_oc_status.needSyncPPS = 1;
				}
				g_oc_status.fixed = 1;
				g_ctlStatus.basetemp = g_ctlStatus.temp;
			}
		}
		else
		{
			g_oc_status.fixed = 0;
			g_oc_status.fixedCT = 0;
		}
		float tmpdacd = kalmantmp*g_Ocxo_info.adj_step;
		double freq = 10000000.0;
		double tmpDac = 0;
		DBGI("org=%.2f filter=%.2f name=%s\n", min, kalmantmp, g_Ocxo_info.ocxo_info);
		freq = freq + min * 0.00001f;
		kalmantmp = g_Ocxo_info.dacValue;
		tmpdacd = kalmantmp - tmpdacd;
		if(tmpdacd > 0 && tmpdacd < 1000000) {
			tmpDac = tmpdacd;
		}
		else 
		{
				DBGI("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! error.... %lf\n",min);
				if(kalman.seconds>5000) {
					g_init = 20;
					g_oc_status.fixed = 0;
					tmpDac = g_Ocxo_info.dacValue;
				} else if(kalman.seconds>1000) {
					g_init = 20;
					g_oc_status.fixed= 0;
					tmpDac = 300000;
				}
		}
		float tmpOffset = g_oc_status.historyDatas[g_oc_status.historyIdx];
		g_oc_status.historyDatas[g_oc_status.historyIdx++] = avg;
		g_oc_status.historySum+= avg;
		g_oc_status.history10MinSum += avg;
		if(g_oc_status.history10MinOk)
		{
			int16_t tmp = g_oc_status.historyIdx - 21;
			if(tmp < 0)
			{
				tmp = tmp + 120;
			}
			g_oc_status.history10MinSum -= g_oc_status.historyDatas[tmp];
		}
		
		if(g_oc_status.historyOk)
		{
			g_oc_status.historySum-=tmpOffset;
		}
		//10 min
		if(g_oc_status.historyIdx == 20)  
		{
			g_oc_status.history10MinOk = 1;
		}
		if(g_oc_status.historyIdx >= 120)
		{
			g_oc_status.historyIdx = 0;
			g_oc_status.historyOk = 1;
		}
		double realOffset = 0;
		DBGI("temp=%.2f freq=%lf sec=%lld yuce=%.2f haveLock=%d\n",g_ctlStatus.temp,freq,kalman.seconds,kalman.yuceNoise, g_oc_status.fixed);
		
		if(g_oc_status.historyOk)
		{
			realOffset = g_oc_status.historySum / 120.0f;
		}
		double realFreq = 10000000.0 + realOffset * 0.00001;
		g_oc_status.offset1Hour = realOffset;
		if(g_oc_status.historyOk)
		{
			DBGI("1 hour realFreq=%lf offset=%f sum=%.1f\n",realFreq, realOffset, g_oc_status.historySum);
		}
		float fixoff = 0;
		float fixlong = 0;
		if(g_oc_status.history10MinOk)
		{
			realOffset = g_oc_status.history10MinSum / 20.0f;
			fixoff = realOffset / g_Controller_config.offsetFix;
			//对积分结果限幅
			if(fixoff > 10) fixoff = 10;
			if(fixoff < -10) fixoff = -10;
		}
		g_oc_status.offset10Min = realOffset;
		double real10Freq =  10000000.0 + realOffset * 0.00001;
		if(g_oc_status.history10MinOk)
		{
			DBGI("10 min realFreq=%lf  offset=%f sum=%.1f fix=%.1f\n",real10Freq, realOffset, g_oc_status.history10MinSum, fixoff);
		}

		if(!g_oc_status.stopControl)
		{
			if(g_gpsStatus.valid)
			{
				//if(g_oc_status.fixed)
				if(1)
				{
					//sumtdc += tdc;
					fixlong = ((float)tdc / g_Controller_config.tdctime/10.0f) * ADC2PS;
					if(fixlong > 20) fixlong = 20;
					if(fixlong < -20) fixlong = -20;
					DBGI("sumtdc = %d %.1f\n", tdc, fixlong);
				}
				g_Ocxo_info.dacValue = tmpDac - fixoff*g_Ocxo_info.adj_step - fixlong*g_Ocxo_info.adj_step;
				//
				/*extern uint32_t preValue;
				float increat = pid_calculate_increment(&pid, preValue, 30);
				DBGI("adc=%d increment =%.1f\n", preValue, increat);
				g_Ocxo_info.dacValue -= increat;*/
				setDacValue(g_Ocxo_info.dacValue,g_oc_status.fixed);
				historylog();
				//TempAndAge();
				ocxo_log();
			}
			else
			{
				double tempadj = 0;
				tempadj = g_Ocxo_info.temp * (g_ctlStatus.temp-g_ctlStatus.basetemp);
				g_Ocxo_info.dacValue += tempadj;
				DBGI("temp adj = %lf\n", tempadj);
				setDacValue(g_Ocxo_info.dacValue,g_oc_status.fixed);
			}
		}
		else
		{
			g_oc_status.history10MinOk = 0;
			g_oc_status.historyOk = 0;
			g_oc_status.historyIdx = 0;
			g_oc_status.history10MinSum = 0;
			g_oc_status.historySum = 0;
		}
		kalmanUpdateControl(g_Ocxo_info.dacValue,(int32_t)min,g_sampleCount);
		g_oc_status.controlCount = 0;
		g_oc_status.controlSum = 0;
	}
}

